#include "IMU.h"
#include "hardwareDef.h"
#include "simpleMath.h"
#include "PID.h"

uint8_t rxAOA[8] = {};
uint8_t aoaUpdate = 0;
float aoaNow;

extern float trueAOA;//返回真实AOA数据，单位为deg
extern float feedback[7];//横滚速率0、俯仰速率1、偏航速率2、AOA 3、横滚角4、俯仰角5、横向加速度6，归一法
extern unsigned long tStamp[3];//用于收集各传感器更新事件戳，以此判断是否失效,0遥控，1攻角，2陀螺仪

#include <SPI.h>
#include "SC16IS752.h"
SC16IS752 spiuart = SC16IS752(SC16IS750_PROTOCOL_SPI, spiCS);
#include "JY901.h"

void INIT_AOA()
{
  spiuart.begin(115200, 115200); //baudrate setting
}

void RX_AOA()
{
  static uint8_t rxRaw[8] = {};//建立缓存区
  static uint8_t index;//接收队列序号
  static uint8_t state;//接收状态
  while (spiuart.available(SC16IS752_CHANNEL_A))
  {
    uint8_t c = spiuart.read(SC16IS752_CHANNEL_A);
    //Serial.write(c);
    switch (c)
    {
      case ':'://接收到冒号，进入预备状态
        state = 1;
        break;

      case '\r'://读到回车进入转录状态
        state = 3;
        break;

      case '\n'://读到回车进入待机状态
        state = 0;
        break;
    }

    switch (state)
    {
      case 0://待机状态
        index = 0;
        break;

      case 1://预备状态
        state = 2;//进入读取状态
        break;

      case 2://读取状态
        rxRaw[index] = c;//缓存区接收数据
        index ++;//队列序号递增
        break;

      case 3://转录状态
        for(uint8_t i = 0; i < index; i ++)
        {
          rxAOA[i] = CHAR2HEX(rxRaw[i]);//转录至AOA
          if (rxAOA[i] == 0xFF)
          {
            state = 0;
          }
          rxRaw[i] = 0xFF;//将缓存区刷为0xFF，以便下次判断误码
        }
        aoaUpdate = index;
        TSD_AOA();
        state = 0;
        break;
    }

    if (index >= 8)
    {
      state = 0;
    }
  }
}

void RX_GYRO()
{
  while (spiuart.available(SC16IS752_CHANNEL_B))
  {//横滚速率0、俯仰速率1、偏航速率2、AOA反馈3、横滚角度4、俯仰角度5，归一法
    JY901.CopeSerialData(spiuart.read(SC16IS752_CHANNEL_B));
    tStamp[2] = millis();
  }
  feedback[0] = (float)JY901.stcGyro.w[1] / 16.384 / rrMax;
  if (JY901.stcGyro.w[0] >= 0)
  {
    feedback[1] = (float)JY901.stcGyro.w[0] / 16.384 / prMax;
  }
  else
  {
    feedback[1] = -(float)JY901.stcGyro.w[0] / 16.384 / prMin;
  }
  feedback[2] = -(float)JY901.stcGyro.w[2] / 16.384 / yrMax;

  feedback[4] = (float)JY901.stcAngle.Angle[1]/182.044 / rMax;
  if (JY901.stcAngle.Angle[0] >= 0)
  {
    feedback[5] = (float)JY901.stcAngle.Angle[0]/182.044 / pMax;
  }
  else
  {
    feedback[5] = -(float)JY901.stcAngle.Angle[0]/182.044 / pMin;
  }
  feedback[6] = (float)JY901.stcAcc.a[0]/2048 / ayMax;
}

void TSD_AOA()
{//AOA传感器数据读取
  uint8_t i = 0, j = 1;
  uint8_t state = 0;
  aoaNow = 0;
  while (aoaUpdate)
  {
    if (rxAOA[i] == '.')
    {
      state = 1;
    }

    switch (state)
    {
      case 0:
        aoaNow *= 10;
        aoaNow += rxAOA[i];
        break;

      case 1:
        state = 2;
        break;

      case 2:
        aoaNow += float(rxAOA[i]) * pow(10, -j);
        j ++;
        break;
    }
    i ++;
    aoaUpdate --;
  }

  float aoa;
  //TH-Heading-clamp01((TH-Heading)>180)*360+clamp01((TH-Heading)<-180)*360
  if ((aoaNow-aoaZero)>180)
  {
    aoa = aoaNow-aoaZero - 360;
  }
  else if ((aoaNow-aoaZero)<-180)
  {
    aoa = aoaNow-aoaZero + 360;
  }
  else
  {
    aoa = aoaNow-aoaZero;
  }
  if (aoa >= 0)
  {
    feedback[3] = aoa / aoaMax;
  }
  else
  {
    feedback[3] = - aoa / aoaMin;
  }
  trueAOA = aoa;
  tStamp[1] = millis();
}
